
#!/usr/bin/env python3
#coding=utf-8
import time
from Rosmaster_Lib import Rosmaster
from ipywidgets import interact
import ipywidgets as widgets
import math
import pygame #
import matplotlib.pyplot as plt


import numpy as np
pygame.init()
pygame.joystick.init()

# 手柄打开
joystick = pygame.joystick.Joystick(0)
# 手柄初始化
joystick.init()
xxx=0
yyy=0
theta=0
theta_out=0
B=110e-3  #轴距
l=127e-3   #轮距

#舵机中值
midtheta1=92
midtheta2=92
midtheta3=112
midtheta4=90

x1_old ,x2_old, x3_old ,x4_old=0,0,0,0
all_theta=0
# Rosmaster库
bot = Rosmaster()
# 打开接受数据进程
bot.create_receive_threading()

#增量式PID
class IncrementalPID:    
    def __init__(self, P, I, D):        
        self.Kp = P        
        self.Ki = I        
        self.Kd = D        
        self.last_error = 0.0        
        self.output = 0.0        
        self.integral = 0.0    
    def calculate(self, setpoint, feedback_value):        
        error = setpoint - feedback_value        
        delta_error = error - self.last_error        
        self.integral += error        
        self.output += self.Kp * delta_error + self.Ki * error + self.Kd * delta_error        
        self.last_error = error        
        return self.output
    
#角度显示函数
def limittheta(theta,limitnum):
    if theta>=limitnum:
        theta=limitnum
    elif theta<=-limitnum:
        theta=-limitnum
    else : theta=theta
    return theta 
##PWM限制函数
def limitpwm(pwm):
    if pwm>=50:
        pwm=50
    elif pwm<=-50:
        pwm=-50
    else : pwm=pwm
    return pwm 

#PWM角度输出函数
def pwm_servo(S1, S2, S3, S4):
    
    bot.set_pwm_servo(1, S1)
    bot.set_pwm_servo(2, S2)
    bot.set_pwm_servo(3, S3)
    bot.set_pwm_servo(4, S4)

#双阿克曼转向
def doubleAKM(thetaf):
    thetar=-thetaf
    Q=B*(math.atan(thetaf)-math.tan(thetar))/(2*l)
    thetafl=math.degrees(math.atan(math.tan(thetaf)/(1-Q)))#?????????thetafl
    thetafr=math.degrees(math.atan(math.tan(thetaf)/(1+Q)))#?????????thetafr
    thetarl=math.degrees(math.atan(math.tan(thetar)/(1-Q)))#?????????thetarl
    thetarr=math.degrees(math.atan(math.tan(thetar)/(1+Q)))#?????????thetarr
    thetafl=thetaf
    thetafr=thetaf
    thetarl=thetar
    thetarr=thetar

    get_thetafl=midtheta1+limittheta(thetafl,20)
    get_thetafr=midtheta2+limittheta(thetafr,20)
    get_thetarl=midtheta3+limittheta(thetarl,20)
    get_thetarr=midtheta4+limittheta(thetarr,20)
    
    pwm_servo(get_thetafl,get_thetafr,get_thetarl,get_thetarr)

#单阿克曼转向模式
def danAKM(thetaf):
    thetar=-thetaf
   
    thetarl=0
    thetarr=0
    ##??????????? 
    get_thetafl=midtheta1+limittheta(thetaf,20)
    get_thetafr=midtheta2+limittheta(thetaf,20)
    get_thetarl=midtheta3+limittheta(thetarl,20)
    get_thetarr=midtheta4+limittheta(thetarr,20)
    pwm_servo(get_thetafl,get_thetafr,get_thetarl,get_thetarr)
#斜移模式
def xieyi(thetaf):
    get_thetafl=midtheta1+limittheta(thetaf,90)
    get_thetafr=midtheta2+limittheta(thetaf,90)
    get_thetarl=midtheta3+limittheta(thetaf,90)
    get_thetarr=midtheta4+limittheta(thetaf,90)
#横移模式
def hengyi(thetaf):
    get_thetafl=midtheta1+limittheta(thetaf,90)
    get_thetafr=midtheta2+limittheta(-thetaf,90)
    get_thetarl=midtheta3+limittheta(-thetaf,90)
    get_thetarr=midtheta4+limittheta(thetaf,90)
    pwm_servo(get_thetafl,get_thetafr,get_thetarl,get_thetarr)
#原地转模式
def lingzhuan():
    thetaf=47.5
    get_thetafl=midtheta1+thetaf
    get_thetafr=midtheta1-thetaf
    get_thetarl=midtheta1-thetaf
    get_thetarr=midtheta1+thetaf
    pwm_servo(get_thetafl,get_thetafr,get_thetarl,get_thetarr)
#模式切换
def switch(lang):
    if lang == 0:
        doubleAKM(limittheta(theta_out/2,15))
        bot.set_motor(int(vv),int(vv),int(vv),int(vv))

    # elif lang == 1:
    #     danAKM(limittheta(theta_out/2,30))
    #     bot.set_motor(int(vv),int(vv),int(vv),int(vv))
    elif lang == 1:
        xieyi(limittheta(theta_out/2,90))
        bot.set_motor(int(vv),int(vv),int(vv),int(vv))
    elif lang == 2:
        hengyi(90)
        bot.set_motor(int(vv),int(-vv),int(-vv),int(vv))
    elif lang == 3:
         danAKM(limittheta(theta_out/2,30))
         bot.set_motor(int(vv),int(vv),int(vv),int(vv))    
    elif lang == 4:
        lingzhuan()
        bot.set_motor(int(vv),int(-vv),int(vv),int(-vv))
    
    elif lang == 5:
        doubleAKM(0)
        bot.set_motor(0, 0, 0, 0)





aaa=0
number=0#lvbo cishuo
models=0  #--shuangekeman
           #1--danerkeman
           #2--xieyi
           #3--zhixian
           #4--yuandi
           #5--tingche
do =True          
while do:
    try:
        for event_ in pygame.event.get():#手柄接收线程
            if event_.type == pygame.JOYBUTTONDOWN or event_.type == pygame.JOYBUTTONUP:
                if joystick.get_button(3)==1:
                
                    print("x")
                    aaa=1
                else :aaa=0
                if joystick.get_button(4)==1: 
                
                    print("y")
                if joystick.get_button(2)==1: 
                    do=False
                    print("B")
                if joystick.get_button(0)==1: 
            
                    print("A")
                
                if joystick.get_button(10)==1:
                    models=models+1 #模式+
                if joystick.get_button(11)==1:
                    models=models-1 #模式-
            elif event_.type == pygame.JOYAXISMOTION:
                xxx=joystick.get_axis(0)
                yyy=-joystick.get_axis(1)
                qianpwm=joystick.get_axis(4) #左扳机加速
                houpwm=joystick.get_axis(5)  #右扳机减速
                if 0.5>xxx and xxx>-0.5:
                    if 0.5>yyy and yyy>-0.5:
                        yyy=0
                        xxx=0
                
                if xxx==0:
                    theta=0
            
                if xxx!=0:
                    if yyy==0:
                        theta=90 
                    if yyy!=0:
                        theta=(math.atan(xxx/yyy))*57.3
                    else :
                        xxx=0
                        yyy=0
            else:
                qianpwm=0
                houpwm=0
        all_theta=theta+all_theta  
        number=number+1
        
        if number==5:
            theta_out=all_theta/5
            all_theta=0
            number=0
        
        if models>5:
            models=5
        if 0>models:
            models=0
        # print("models:%d",models)    
        # print(theta_out)
        # print("stop:%d",aaa)
        roll,pitch, yaw =bot.get_imu_attitude_data(True)  #获取姿态角
        print("roll,pitch, yaw",roll,pitch, yaw) 
        
        x1_out,x2_out,x3_out,x4_out=bot.get_motor_encoder() #获取编码器数据
        vv=(qianpwm-houpwm)*20
        m1, m2, m3, m4 = bot.get_motor_encoder()
        print("encoder:", m1, -m2, m3, -m4)


        #print("q:%d,h:%d,vv:%d",qianpwm,houpwm,vv)
        # print("bianmaqi:fl:%d,fr:%d,rl:%d,rr:%d\n",x1_out,-x2_out,-x3_out,x4_out)
        v1,v2,v3,v4=100,100,100,100

        
        if aaa==0:
            switch(models)
            #bot.set_motor(int(PWM1), int(PWM2), int(PWM3), int(PWM4))
        else :
             bot.set_motor(0, 0, 0, 0) 
    except KeyboardInterrupt:

        do=False
        break
                
bot.set_motor(0, 0, 0, 0)
del bot
 




